#!/usr/bin/env python

import time
import random
import rospy
import carla
from carla_msgs.msg import CarlaWorldInfo
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
import carla_common.transforms as trans
from carla_msgs.msg import CarlaEgoVehicleControl
from nav_msgs.msg import Odometry
import math
import geometry_msgs
import numpy as np
import controller2d
from derived_object_msgs.msg import ObjectArray, Object
import threading
from std_msgs.msg import Header
from shape_msgs.msg import SolidPrimitive

parked_locations = [
    carla.Transform(carla.Location(x=-28.7884, y=-27.1124, z=0.239271),
                    carla.Rotation(yaw=-178.76)),
    carla.Transform(carla.Location(x=-28.7556, y=-32.7723, z=0.239317),
                    carla.Rotation(yaw=179.173))
]

goal_locations = [-28.791819,  -29.965797]
# goal_locations = carla.Transform(carla.Location(
#     x=-26.855478, y=-32.729084, z=0.197679), carla.Rotation(yaw=-179.712860))


init_pos = carla.Transform(carla.Location(
    x=-36.211590, y=-33.486397, z=0.333945), carla.Rotation(yaw=-114.764809))
# init_pos = carla.Transform(carla.Location(
#     x=-35.809448, y=-23.090153, z=0.242889), carla.Rotation(yaw=-88.704109))


def eucliDist(A, B):
    return math.sqrt(sum([(a - b)**2 for (a, b) in zip(A, B)]))


class CarlaParkVehicle():
    """
    class responsable of:
        -spawning 3 vehicles of which one ego
        -interact with ROS and carla server
        -destroy the created objects
        -execute the parking manoeuvre
    """

    def __init__(self):
        """
        construct object CarlaParkVehicle with server connection and
        ros node initiation
        """
        self.client = carla.Client('localhost', 2000)
        self.client.set_timeout(10.0)
        self.client.load_world('Town05')
        self.world = self.client.get_world()
        self.actor_list = []
        blueprint_library = self.world.get_blueprint_library()
        rospy.init_node('park_vehicle', anonymous=True)
        self.role_name = "ego_vehicle"
        #
        self.object_publisher = rospy.Publisher(
            "/carla/" + self.role_name + "/objects", ObjectArray,  queue_size=10)

        # Subscription Hybrid A Path
        rospy.Subscriber("/xsj/planning/dynamic_waypoints_vis",
                         Path, self.local_waypoints)

        # create ego vehicle
        bp = random.choice(blueprint_library.filter('vehicle.tesla.model3'))
        self.vehicle = self.world.spawn_actor(bp, init_pos)
        self.actor_list.append(self.vehicle)
        self.ob_list = []

        # create 2 parked vehicles
        for pos in parked_locations:
            v = self.world.spawn_actor(bp, pos)
            self.actor_list.append(v)
            self.ob_list.append(v)

        #
        self.path = []
        self.one = False
        self.complete = False
        self.alpha_prev = 0
        self.waypoint_list = []
        rospy.loginfo('created %s' % self.vehicle.type_id)

    def local_waypoints(self, waypoints):
        if self.one == False:
            T = 100
            for point in waypoints.poses:
                pi_location = carla.Location(
                    x=point.pose.position.x, y=point.pose.position.y, z=0.5)
                self.world.debug.draw_point(
                    pi_location, size=0.05, color=carla.Color(r=255), life_time=T)
                self.waypoint_list.append(
                    [point.pose.position.x,  point.pose.position.y, 0.1])
            self.one == True
        #
        if len(self.waypoint_list) > 0:
            if self.complete == False:
                res = self.park()
                if res == True:
                    time.sleep(1.0)
                    self.vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=-0.4,
                                                                    brake=0.0, reverse=False))
                    time.sleep(1.9)
                    self.vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=0.0,
                                                                    brake=0.0, reverse=True))
                    time.sleep(2)
                    self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0,
                                                                    brake=1.0, reverse=True))
                    rospy.loginfo("parking complete")
                    self.complete = True
                
        return

    def park(self):
        # Game Loop
        current_x = self.vehicle.get_transform().location.x
        current_y = self.vehicle.get_transform().location.y
        current_yaw = math.radians(self.vehicle.get_transform().rotation.yaw)
        current_speed = 0.1

        controller = controller2d.Controller2D(self.waypoint_list)
        # Update the other controller values and controls
        controller.update_values(current_x, current_y,
                                 current_yaw, current_speed)
        controller.update_controls()
        cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
        # print("cmd_throttle: ", cmd_throttle)
        # print("cmd_steer: ", cmd_steer)
        # print("cmd_brake: ", cmd_brake)

        self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=cmd_steer,
                                                        brake=cmd_brake, reverse=True))

        time.sleep(0.5)

        if eucliDist(goal_locations, [current_x, current_y]) < 1.0:
            self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=cmd_steer,
                                                            brake=1.0, reverse=True))
            return True

        return False

    def get_msg_header(self, frame_id=None, timestamp=None):
        """
        Get a filled ROS message header
        :return: ROS message header
        :rtype: std_msgs.msg.Header
        """
        header = Header()
        header.frame_id = frame_id
        header.stamp =  rospy.Time.now()
        return header

    def destroy(self):
        """
        destroy all the actors
        """
        print('destroying actors')
        for actor in self.actor_list:
            actor.destroy()
        print('done.')

    def run(self):
        """
        main loop
        """
        while True:
            ros_objects = ObjectArray()
            ros_objects.header.frame_id = "map"
            ros_objects.header.stamp = rospy.Time.now()
            for  actor in self.ob_list:
                obj = Object(header=self.get_msg_header("map"))
                obj.pose = trans.carla_transform_to_ros_pose(actor.get_transform()) 
                obj.twist = trans.carla_velocity_to_ros_twist(actor.get_velocity(), actor.get_angular_velocity())
                obj.accel = trans.carla_acceleration_to_ros_accel(actor.get_acceleration())
                obj.shape.type = SolidPrimitive.BOX
                obj.shape.dimensions.extend([
                    actor.bounding_box.extent.x * 2.0,
                    actor.bounding_box.extent.y * 2.0,
                    actor.bounding_box.extent.z * 2.0])
                ros_objects.objects.append(obj)

            self.object_publisher.publish(ros_objects)

        try:
            rospy.spin() 
        except rospy.ROSInterruptException:
            pass


# ==============================================================================
# -- main() --------------------------------------------------------------------
# ==============================================================================

def main():
    """
    Main function
    """
    ego_vehicle = CarlaParkVehicle()
    try:
        ego_vehicle.run()
    finally:
        if ego_vehicle is not None:
            ego_vehicle.destroy()


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
